#include "gravity_cal.h"
#include <math.h>

// 机械臂长度   单位：m
#define L_BACKARM           1
#define L_FOREARM           1
// 机械臂质量   单位：kg
#define M_BACKARM           1
#define M_FOREARM           1
// 重力常数
#define G                   9.81

/**
 * @brief 计算补偿力矩
 * 
 * @param cdata 
 */
void g_torque_cal(g_dataTypeDef cdata)
{
        // 提取关节角度（假设已存储在结构体中）
    // 注意：实际代码中可能需要从编码器读取角度值
    float theta1 = cdata.theta1; // 大臂角度
    float theta2 = cdata.theta2; // 小臂角度
    
    // 计算重力补偿扭矩
    // 关节1（大臂）的补偿扭矩
    cdata.g_tor_backarm = (M_BACKARM * G * cdata.Cp_backarm + 
                          M_FOREARM * G * L_BACKARM) * cos(theta1) +
                         M_FOREARM * G * cdata.Cp_forearm * cos(theta1 + theta2);
    
    // 关节2（小臂）的补偿扭矩
    cdata.g_tor_forearm = M_FOREARM * G * cdata.Cp_forearm * cos(theta1 + theta2);
    
    // 注意：这里计算的是补偿扭矩，实际电机输出应该是这个值的相反数
    // 因为需要抵消重力产生的扭矩
}